/*
 * MOTOR.c
 *
 * Created: 29.10.2012 15:05:06
 *  Author: bardbakk
 */ 
#define F_CPU 8500000

#include <stdlib.h>
#include <avr/io.h>
#include <avr/delay.h>
#include <avr/interrupt.h>

#include "definitions.h"
#include "USART.h"
#include "REG.h"
#include "MOTOR.h"
#include "DAC.h"

#define MOTOR_ENABLE PA4
#define MOTOR_DIR PA3 
#define ENCODER_ENABLE PA7
#define ENCODER_SELECT PA5 
#define ENCODER_RESET PA6 


static int encoder_max;
static int reference = 0;


void MOTOR_set_speed(int val)
{
	//Set motor direction
	uint8_t speed;
	if (val < 0)
	{
		BIT_CLEAR(PORTA, MOTOR_DIR);
	}
	else
	{
		BIT_SET(PORTA, MOTOR_DIR);	
	}
	
	//Set motor value 0-5V
	if(abs(val) > 100)
	{
		val = 100;
	}
	
	speed = abs(val)*255/100;
	DAC_write(speed);
}


/*!
 * @function	MOTOR_MJ2_bit_reverse
 * @discussion	Reverses a byte
 * @param		byte	The byte to be reversed..
 * @result		The reversed byte.
 */	
static uint8_t MOTOR_MJ2_bit_reverse(uint8_t byte)
{
	uint8_t i;
	uint8_t reversed = 0;
	
	for(i = 0; i<8; i++)
	{

		if(BIT_CHECK(byte, i))
		{
			reversed += 1 << (7-i);
		}
		
	}
	return reversed;
}


/*!
 * @function	MOTOR_read_encoder.
 * @discussion	Reads the encoder.
 * @result		The current position.
 */	
static int MOTOR_read_encoder()
{
	int position = 0;
	uint8_t byte;

	//Read high byte
	BIT_CLEAR(PORTA, ENCODER_SELECT);
	_delay_us(20);
	byte = PINC;
	
	position = MOTOR_MJ2_bit_reverse(byte);
	position = position << 8;

	//Read low byte
	BIT_SET(PORTA, ENCODER_SELECT);
	_delay_us(20);
	byte = PINC;
	position += MOTOR_MJ2_bit_reverse(byte);

	return position;
}

void MOTOR_init()
{
	//DAC_init();
	REG_param_load();
	
	// Set PortA (MJ1) as output
	DDRA = 0xFF;
	
	// Set PortC (MJ2) as input
	DDRC = 0x0;
	
	//Enable motor
	BIT_SET(PORTA, MOTOR_ENABLE);
	
	//Enable encoder
	BIT_CLEAR(PORTA, ENCODER_ENABLE);
	
	//Calibrate encoder	
	BIT_CLEAR(DDRD, PD7);
	
	MOTOR_set_speed(-40);
	_delay_ms(1500);
	
	//Set encoder 0 position
	BIT_CLEAR(PORTA, ENCODER_RESET);
	BIT_SET(PORTA, ENCODER_RESET);
	
	MOTOR_set_speed(40);
	

	while(BIT_CHECK(PIND, PD7))
	{
		//Wait for the motor to stop
	}
			
	//Set encoder max position	
	encoder_max = MOTOR_read_encoder();
		
	MOTOR_set_speed(0);
}


void MOTOR_set_reference(int val)
{
	reference = val;
}

void MOTOR_update_input(float dt)
{
	int error, u;
	error = reference - ((MOTOR_read_encoder())/(encoder_max/200) - 100);
	u = REG_controller(error, dt);
	
	MOTOR_set_speed(u);
}
